#!/usr/bin/env python
# -*- coding: UTF-8 -*-
"""
@Project ：刘骏轩2025讯飞
@File ：GetCvPicture.py
@Author ：ljx
@Date ：2025/7/27 下午8:05
"""

import sys

sys.path.insert(0, '/home/ucar/cv_bridge_ws/devel/lib/python3/dist-packages')

import rospy
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2
import queue
import threading

class PicGetter:
    def __init__(self):
        self.Image_queue = queue.Queue(maxsize=1)
        self.pic_sub = None
        self.bridge = CvBridge()
        self.lock = threading.Lock()

    def callback(self, data):
        with self.lock:  # 确保线程安全
            while not self.Image_queue.empty():  # 循环检查队列是否为空
                try:
                    self.Image_queue.get_nowait()  # 非阻塞式移除队首元素
                except queue.Empty:  # 队列为空则退出
                    break
            self.Image_queue.put(data)

    def Get(self):
        ros_image = None
        try:
            with self.lock:
                ros_image = self.Image_queue.get(timeout=0.1)
        except queue.Empty:
            rospy.logdebug("图像队列为空")
            return None
        try:
            cv_img = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
            return cv_img

        except Exception as e:
            rospy.loginfo(f"图像转换失败: {e}")
            return None

    def start(self):
        self.pic_sub = rospy.Subscriber('/picture', Image, self.callback)
        rospy.loginfo("图片订阅已启动")


if __name__ == '__main__':
    rospy.init_node('GetImageDemo', anonymous=True)
    cap = PicGetter()
    cap.start()

    rate = rospy.Rate(30)

    while not rospy.is_shutdown():
        img = cap.Get()
        if img is not None:
            cv2.imshow('img',img)
            cv2.waitKey(1)
            pass
        rate.sleep()
